﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using componentXtra;
using System.Drawing.Drawing2D;

namespace Simulador_Quadrotor
{
    public partial class PIDxy : Form
    {
        PID pidAltura = null;
        PID pidRoll = null;
        PID pidPitch = null;
        PID pidYaw = null;
        PID pidX = null;
        PID pidY = null;
        List<double> tiempo = null;
        Dinamica dinamica = null;
        double tiempoTotal = 0;
        double tiempoFinal = 0;
        double tamanoPaso;
        double[] rot = null;
        Estado estadoFinal = null;
        Estado estadoActual = null;
        ControlMotor controlMotor = null;
        ConfiguracionPurebaPID configuracionPurebaPID = null;

        public PIDxy(ConfiguracionPurebaPID configuracionPurebaPID)
        {
            InitializeComponent();
            Inicializa(configuracionPurebaPID);
            this.configuracionPurebaPID = configuracionPurebaPID;
            this.numericUpDownXd.Value = (decimal)configuracionPurebaPID.X.kd;
            this.numericUpDownXi.Value = (decimal)configuracionPurebaPID.X.ki;
            this.numericUpDownXp.Value = (decimal)configuracionPurebaPID.X.kp;

            this.numericUpDownYd.Value = (decimal)configuracionPurebaPID.Y.kd;
            this.numericUpDownYi.Value = (decimal)configuracionPurebaPID.Y.ki;
            this.numericUpDownYp.Value = (decimal)configuracionPurebaPID.Y.kp;
        }

        private void Inicializa(ConfiguracionPurebaPID configuracionPurebaPID)
        {
            pidAltura = new PID();
            pidAltura.Kd = configuracionPurebaPID.Altura.kd;
            pidAltura.Ki = configuracionPurebaPID.Altura.ki;
            pidAltura.Kp = configuracionPurebaPID.Altura.kp;
            pidAltura.Tp = configuracionPurebaPID.TPaso;

            pidPitch = new PID();
            pidPitch.Kd = configuracionPurebaPID.Pitch.kd;
            pidPitch.Ki = configuracionPurebaPID.Pitch.ki;
            pidPitch.Kp = configuracionPurebaPID.Pitch.kp;
            pidPitch.Tp = configuracionPurebaPID.TPaso;

            pidRoll = new PID();
            pidRoll.Kp = configuracionPurebaPID.Roll.kp;
            pidRoll.Ki = configuracionPurebaPID.Roll.ki;
            pidRoll.Kd = configuracionPurebaPID.Roll.kd;
            pidRoll.Tp = configuracionPurebaPID.TPaso;

            pidYaw = new PID();
            pidYaw.Kp = configuracionPurebaPID.Yaw.kp;
            pidYaw.Ki = configuracionPurebaPID.Yaw.ki;
            pidYaw.Kd = configuracionPurebaPID.Yaw.kd;
            pidYaw.Tp = configuracionPurebaPID.TPaso;

            tiempo = new List<double>();
            estadoActual = new Estado(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
            estadoFinal = new Estado(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
            rot = new double[] { 0, 0, 0, 0 };
            dinamica = new Dinamica(rot, estadoActual);
            controlMotor = new ControlMotor();

            pidX = new PID();
            pidX.Tp = configuracionPurebaPID.TPaso;

            pidY = new PID();
            pidY.Tp = configuracionPurebaPID.TPaso;

            this.tamanoPaso = configuracionPurebaPID.TPaso;
            this.tiempoFinal = configuracionPurebaPID.Tiempo;
            this.tiempo.Clear();
            this.tiempoTotal = 0;
        }

        private void toolStripButton1_Click(object sender, EventArgs e)
        {
            Inicializa(this.configuracionPurebaPID);
            GraficaX.ClearGraphs();
            GraficaY.ClearGraphs();

            this.tiempoFinal = (double)this.numericUpDownTiempo.Value;
            this.estadoFinal.X = (double)this.numericUpDownXDeseada.Value;
            this.estadoFinal.Y = (double)this.numericUpDownYDeseado.Value;
            //configuramos el PID
            this.pidX.Kp = (double)this.numericUpDownXp.Value;
            this.pidX.Ki = (double)this.numericUpDownXi.Value;
            this.pidX.Kd = (double)this.numericUpDownXd.Value;

            this.pidY.Kp = (double)this.numericUpDownYp.Value;
            this.pidY.Ki = (double)this.numericUpDownYi.Value;
            this.pidY.Kd = (double)this.numericUpDownYd.Value;

            double controlZ;
            double controlPitch;
            double controlRoll;
            double controlYaw;
            double controlX;
            double controlY;
            //EMpezamos la simulacion
            int numeroPasos = 0;
            int.TryParse((this.tiempoFinal / this.tamanoPaso).ToString(), out numeroPasos);
            for (int i = 0; i < numeroPasos; i++)
            {
                this.tiempo.Add(tiempoTotal);
                //calculamos los estados
                this.estadoActual = this.dinamica.IntegraPaso(ref this.tiempoTotal, this.tamanoPaso, this.rot);
                //controlamos la x
                controlX = this.pidX.Control(this.estadoActual, this.estadoFinal, this.estadoFinal.X - this.estadoActual.X);
                controlX = controlMotor.Saturar(controlX, 0.1, -0.1);
                //controlamos la Y
                controlY = this.pidY.Control(this.estadoActual, this.estadoFinal, this.estadoFinal.Y - this.estadoActual.Y);
                controlY = controlMotor.Saturar(controlY, 0.1, -0.1);
                //controlamos la altura
                controlZ = this.pidAltura.Control(this.estadoActual, this.estadoFinal, this.estadoFinal.Z - this.estadoActual.Z);
                //controlamos el Roll
                controlRoll = this.pidRoll.Control(this.estadoActual, this.estadoFinal, controlY - this.estadoActual.phi);
                //controlamos el Pitch              
                controlPitch = this.pidPitch.Control(this.estadoActual, this.estadoFinal, (-controlX) - this.estadoActual.theta);

                //controlamos el Yaw
                controlYaw = this.pidYaw.Control(this.estadoActual, this.estadoFinal, this.estadoFinal.psi - this.estadoActual.psi);
                //controlamos las salidas de los rotores
                this.rot = this.controlMotor.Controlar(controlZ, controlRoll, controlPitch, controlYaw, this.estadoFinal.Z - this.estadoActual.Z);
                //Pintamos los valores
                this.PintaGrafica(GraficaX, this.tiempoTotal, this.estadoActual.X);
                this.PintaGrafica(GraficaY, this.tiempoTotal, this.estadoActual.Y);


            }

        }
        private void PintaGrafica(XYGraph grafica, double tiempo, double valor)
        {

            grafica.AddGraph("", DashStyle.Solid, Color.Red, 0, false);
            grafica.AddValue(0, (float)tiempo, (float)valor);
            grafica.DrawAll();
        }

        private void toolStripButtonOk_Click(object sender, EventArgs e)
        {
            this.configuracionPurebaPID.X.kd = (double)this.numericUpDownXd.Value;
            this.configuracionPurebaPID.X.ki = (double)this.numericUpDownXi.Value;
            this.configuracionPurebaPID.X.kp = (double)this.numericUpDownXp.Value;

            this.configuracionPurebaPID.Y.kd = (double)this.numericUpDownYd.Value;
            this.configuracionPurebaPID.Y.ki = (double)this.numericUpDownYi.Value;
            this.configuracionPurebaPID.Y.kp = (double)this.numericUpDownYp.Value;

            this.Close();
        }

        private void toolStripButtonCancel_Click(object sender, EventArgs e)
        {
            this.Close();
        }



    }
}
